##Realize feqture points matching

import cv2
import numpy as np
import pickle
from scipy.spatial import ConvexHull

# 1. 加载双目图像
img_left  = cv2.imread(r'E:\University\Junior_up\twelfth_xing_huo_bei\Volume_Measure\Using Python\Photograph_Data\Left_view\Left_real_1.jpg', cv2.IMREAD_GRAYSCALE)
img_right = cv2.imread(r'E:\University\Junior_up\twelfth_xing_huo_bei\Volume_Measure\Using Python\Photograph_Data\Right_view\Right_real_1.jpg', cv2.IMREAD_GRAYSCALE)

# 2. 特征点检测与描述符计算
orb = cv2.ORB_create()
keypoints_left, descriptors_left = orb.detectAndCompute(img_left, None)
keypoints_right, descriptors_right = orb.detectAndCompute(img_right, None)

# 3. 特征点匹配
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(descriptors_left, descriptors_right)
matches = sorted(matches, key=lambda x: x.distance)

# 4. 过滤并绘制匹配结果
good_matches = matches[:50]

matched_img = cv2.drawMatches(img_left, keypoints_left, img_right, keypoints_right, good_matches, None)

# cv2.namedWindow('Image')
# cv2.imshow('Image', matched_img)
# key = cv2.waitKey(0)
# if key == ord('q'):
#     cv2.destroyAllWindows()

# 5. 三角测量计算 3D 点
points_left = np.array([keypoints_left[m.queryIdx].pt for m in good_matches], dtype=np.float32)
points_right = np.array([keypoints_right[m.trainIdx].pt for m in good_matches], dtype=np.float32)
# print(points_left)
# print(points_right)

# 6. 双目相机标定参数读入
# 自定义一个类
class Calibration_data:
    def __init__(self, data_dict):
        for key, value in data_dict.items():
            setattr(self, key, value)

# 加载读取 pkl 文件并存入类对象
with open(r'E:\University\Junior_up\twelfth_xing_huo_bei\Volume_Measure\Using Python\Photograph_Data\stereo_calibration_data.pkl', 'rb') as f:
    data = pickle.load(f)

# 创建 Calibration_data 实例，将字典内容存为实例属性
calibration_data = Calibration_data(data)

# 需要相机的投影矩阵 projMatrix1 和 projMatrix2
# 假设 projMatrix1 和 projMatrix2 已知
points_3D = cv2.triangulatePoints(calibration_data.P1, calibration_data.P2, points_left.T, points_right.T)
points_3D /= points_3D[3]  # 转换为齐次坐标
print(points_3D)

##体积计算
points_3D = points_3D[:3].T  # 取前三行并转置为 Nx3 矩阵

# 8. 计算3D点云的凸包并估算体积
hull = ConvexHull(points_3D)
volume = hull.volume
print("Estimated Volume:", volume)